/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-15 03:26:31
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 09:49:08
 * @Description:
 * @Reference:
 */
#include "lci_slam/vilens/pipeline_offline.h"

namespace vilens {

offlinePipeline::offlinePipeline(const std::string &config_path) {
    loadParameter(config_path);
    allocateMemory();
}

void offlinePipeline::loadParameter(const std::string &config_path) {
    pipeline_config.reset(new config());
    pipeline_config->load(config_path);
    pipeline_config->print();
}

void offlinePipeline::allocateMemory() {
    // Lidar
    lidar_preprocess_ptr.reset(new lidarPreprocessor());
    lidar_preprocess_ptr->set_config(pipeline_config->lidar.feature_extract,
                                     pipeline_config->lidar.lidar_type,
                                     pipeline_config->lidar.blind,
                                     pipeline_config->lidar.pfilt_num);

    lidar_process_ptr.reset(new lidarProcessor(pipeline_config));

    // Camera
    camera_process_ptr.reset(new cameraProcessor());
    camera_process_ptr->setConfig();
}

// Imu
void offlinePipeline::pushData(const sensor_msgs::Imu::Ptr &msg) {
    double ang_vel[3];

    // Get IMU samples
    ang_vel[0] = msg->angular_velocity.x;
    ang_vel[1] = msg->angular_velocity.y;
    ang_vel[2] = msg->angular_velocity.z;

    if (this->first_imu_time == 0.) {
        this->first_imu_time = msg->header.stamp.toSec();
    }

    // IMU calibration procedure - do for three seconds
    if (!this->imu_calibrated) {
        static int num_samples = 0;
        static bool print = true;

        if ((imu->header.stamp.toSec() - this->first_imu_time) <
        this->imu_calib_time) {

            num_samples++;

            this->gyroBias(0) += ang_vel[0];
            this->gyroBias(1) += ang_vel[1];
            this->gyroBias(2) += ang_vel[2];

            if(print) {
                std::cout << "Calibrating IMU for " << this->imu_calib_time_ << "
                seconds... "; std::cout.flush(); print = false;
            }

        } else {
            this->gyroBias(0) /= num_samples;
            this->gyroBias(1) /= num_samples;
            this->gyroBias(2) /= num_samples;

            this->imu_calibrated = true;

            std::cout << "done" << std::endl;
            std::cout << "  Gyro biases [xyz]: " << this->gyroBias(0) << ", "
            << this->gyroBias(1) << ", " << this->gyroBias(2) << std::endl
            << std::endl;
        }
    } else {
        // Store into circular buffer
        mtx_data_buffer.lock();
        imu_data_buffer.push_front(msg);
        mtx_data_buffer.unlock();
    }

    return;
}

// Standard_PCL
void offlinePipeline::pushData(const sensor_msgs::PointCloud2::Ptr &msg) {
    mtx_data_buffer.lock();
    lidar_data_buffer.push(msg);
    mtx_data_buffer.unlock();

    // std::cout << lidar_data_buffer.size() << std::endl;

    return;
}

// Image
void offlinePipeline::pushData(
    const std::vector<std::pair<int, sensor_msgs::Image::Ptr>> &msg) {
    return;
}

void offlinePipeline::registerRosVisualizer(ros::NodeHandle &nh) {
    // useViz = true;

    // pubLaserMap = nh.advertise<sensor_msgs::PointCloud2>("/vilens/laser_map",
    // 100000); pubRawLaserScan =
    // nh.advertise<sensor_msgs::PointCloud2>("/vilens/raw_laser_scan", 100000);

    lidar_process_ptr->registerRosVisualizer(nh);
}

imuMeasurements::Ptr offlinePipeline::getImuMeasurement(
    const double &curr_timestamp, const double &prev_timestamp) {
    imuMeasurements::Ptr imuMeas(new imuMeasurements());
    if (prev_timestamp != -1 && prev_timestamp < curr_timestamp) {
        mtx_data_buffer.lock();
        if (imu_calibrated) {
            for (const auto &i : this->imu_data_buffer) {
                double ts = i->header.stamp.toSec();

                if (ts >= prev_timestamp && ts <= curr_timestamp)
                    imuMeas->imus.push_back(i);
            }
        }
        imuMeas->prevTime = prev_timestamp;
        imuMeas->currTime = curr_timestamp;
        imuMeas->accBias = accBias;
        imuMeas->gyroBias = gyroBias;
        mtx_data_buffer.unlock();
    }
    
    return imuMeas;
}


void offlinePipeline::run() {
    // thread
    imuThread.reset(new thread(&offlinePipeline::imuProcess, this));
    lidarThread.reset(new thread(&offlinePipeline::lidarProcess, this));
}


int offlinePipeline::imuProcess() {
    while(1) {
        
    }

    return 1;
}


int offlinePipeline::lidarProcess() {
    while (1) {
        sensor_msgs::PointCloud2::Ptr lidar_msg = nullptr;

        mtx_data_buffer.lock();
        if (!lidar_data_buffer.empty()) {
            lidar_msg = lidar_data_buffer.front();
            lidar_data_buffer.pop();
        }
        mtx_data_buffer.unlock();

        if (lidar_msg) {
            if (last_timestamp_lidar == -1) {
                last_timestamp_lidar = lidar_msg->points[0]
            }
            const double curr_timestamp_lidar = lidar_msg->header.stamp.toSec();
            auto imuMeas =
                getImuMeasurement(curr_timestamp_lidar, last_timestamp_lidar);

            pcl::PointCloud<PointType>::Ptr lidarMeas(new PointCloudXYZI());
            lidar_preprocess_ptr->process(lidar_msg, lidarMeas);

            lidar_process_ptr->process(curr_timestamp_lidar, lidarMeas,
                                       imuMeas);
            last_timestamp_lidar = curr_timestamp_lidar;
        }
    }

    return 0;
}

void offlinePipeline::reset() {}

}  // namespace vilens
